#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sockLib.h>
#include <inetLib.h>
#include <vxWorks.h>
#include "../hardWare.h"

#define UDP_DEBUG

#ifdef UDP_DEBUG
int fpgaUdpDebugFlag = 0;
#define UDP_DEBUG_MSG(...) {if(fpgaUdpDebugFlag) printf(__VA_ARGS__);}
#else
#define UDP_DEBUG_MSG(...)
#endif

int UDPCreate(char * ip, int port)
{
	struct sockaddr_in addr;
	int fd;
	int sockAddrSize = sizeof(struct sockaddr_in);

	bzero((char *) &addr, sockAddrSize);
	addr.sin_family = AF_INET;
	addr.sin_port = htons((unsigned short) port);
	addr.sin_addr.s_addr = inet_addr(ip);

	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == ERROR)
	{
		perror("UDPCreate:socket create error");
		return (ERROR);
	}

	/* Bind socket to local address */
	if (bind(fd, (struct sockaddr *) &addr, sockAddrSize) == ERROR)
	{
		close(fd);
		printf("UDPCreate:socket bind %s:%d error:", ip, port);
		perror("Err:");
		return (ERROR);
	}
	UDP_DEBUG_MSG("UDP Bind %s:%d\n", ip, port);

	return fd;
}

int UDPRecv(int socket, unsigned char * ptr, int msgLen, const char * remoteIP,
		unsigned short remotePort)
{
	struct sockaddr_in clientAddr;
	char recvIP[16];
	int sockAddrSize;
	int ret;

	do
	{
		sockAddrSize = 0;
		bzero((char *) &clientAddr, sizeof(struct sockaddr_in));
		sockAddrSize = sizeof(struct sockaddr_in);
		/*clientAddr.sin_family = AF_INET;
		 clientAddr.sin_port = htons((unsigned short) remotePort);
		 clientAddr.sin_addr.s_addr = inet_addr(remoteIP);*/

		ret = recvfrom(socket, ptr, msgLen, MSG_DONTWAIT,
				(struct sockaddr *) &clientAddr, &sockAddrSize);

		if ((ret > 0) && ((clientAddr.sin_addr.s_addr == inet_addr(remoteIP))||(inet_addr(remoteIP) == INADDR_ANY))
				&& ((clientAddr.sin_port == htons((unsigned short) remotePort))||(remotePort == 0))
				&& (sockAddrSize == sizeof(struct sockaddr_in)))
		{
			inet_ntop (AF_INET, (void *) &(clientAddr.sin_addr), recvIP, INET_ADDRSTRLEN);
			UDP_DEBUG_MSG("UDPRecv %p(%d) from %s:%d \n",ptr, ret, recvIP, htons(clientAddr.sin_port));
			return ret;
		}
		else if(ret > 0)
		{
			inet_ntop (AF_INET, (void *) &(clientAddr.sin_addr), recvIP, INET_ADDRSTRLEN);
			UDP_DEBUG_MSG("UDPRecv Error %p(%d) from %s:%d \n",ptr, ret, recvIP, htons(clientAddr.sin_port));
		}
	}while(ret > 0);

	return 0;
}

int UDPSend(int socket, unsigned char *ptr, int msgLen, char * remoteIP,
		int remotePort)
{
	struct sockaddr_in clientAddr;
	int sockAddrSize = sizeof(struct sockaddr_in);
	int ret;

	if (msgLen == 0)
		return 0;

	bzero((char *) &clientAddr, sockAddrSize);
	clientAddr.sin_family = AF_INET;
	clientAddr.sin_port = htons((unsigned short) remotePort);
	clientAddr.sin_addr.s_addr = inet_addr(remoteIP);
	UDP_DEBUG_MSG(
			"UDPSend %p(%d) to %s:%d \n", ptr, msgLen, remoteIP, remotePort);
	ret = sendto(socket, ptr, msgLen, MSG_DONTWAIT,
			(struct sockaddr *) &clientAddr, sockAddrSize);
	if (ret <= 0)
	{
		UDP_DEBUG_MSG("UDPSend %p(%d) to %s:%d ret %d\n", ptr, msgLen, remoteIP,
				remotePort, ret);
	}

	return ret;
}
